#include "step_motor.h"

static const char *TAG = "STEP_MOTOR";

void step_motor_init()
{

    IoTGpioInit(MOTOR_LEFT_STEP_GPIO);
    IoTGpioSetDir(MOTOR_LEFT_STEP_GPIO, IOT_GPIO_DIR_OUT);
    IoTGpioInit(MOTOR_RIGHT_STEP_GPIO);
    IoTGpioSetDir(MOTOR_RIGHT_STEP_GPIO, IOT_GPIO_DIR_OUT);

    IoTGpioInit(MOTOR_LEFT_DIR_GPIO);
    IoTGpioSetDir(MOTOR_LEFT_DIR_GPIO, IOT_GPIO_DIR_OUT);
    IoTGpioInit(MOTOR_RIGHT_DIR_GPIO);
    IoTGpioSetDir(MOTOR_RIGHT_DIR_GPIO, IOT_GPIO_DIR_OUT);

    MYAPP_LOGI(TAG, "STEP MONITOR INIT SUCCESS: code %d", 0);
}

void step_monitor_set_dir_left()
{
    IoTGpioSetOutputVal(MOTOR_LEFT_DIR_GPIO, 1);
    IoTGpioSetOutputVal(MOTOR_RIGHT_DIR_GPIO, 1);
    MYAPP_LOGI(TAG, "STEP MONITOR SET DIR LEFT SUCCESS: code %d", 0);
}

void step_monitor_set_dir_right()
{
    IoTGpioSetOutputVal(MOTOR_LEFT_DIR_GPIO, 0);
    IoTGpioSetOutputVal(MOTOR_RIGHT_DIR_GPIO, 0);
    MYAPP_LOGI(TAG, "STEP MONITOR SET DIR RIGHT SUCCESS: code %d", 0);
}

void step_monitor_set_dir_up()
{
    IoTGpioSetOutputVal(MOTOR_LEFT_DIR_GPIO, 1);
    IoTGpioSetOutputVal(MOTOR_RIGHT_DIR_GPIO, 0);
    MYAPP_LOGI(TAG, "STEP MONITOR SET DIR UP SUCCESS: code %d", 0);
}

void step_monitor_set_dir_down()
{
    IoTGpioSetOutputVal(MOTOR_LEFT_DIR_GPIO, 0);
    IoTGpioSetOutputVal(MOTOR_RIGHT_DIR_GPIO, 1);
    MYAPP_LOGI(TAG, "STEP MONITOR SET DIR DOWN SUCCESS: code %d", 0);
}



void step_monitor_exe_both_step_params_cylce(int cycles, int sleep_time){
    for (int i = 0; i < cycles; i++)
    {
        step_monitor_exe_both_step_one_cycle(sleep_time);
    }
}

void step_monitor_exe_left_step_params_cylce(int cycles, int sleep_time){
    for (int i = 0; i < cycles; i++)
    {
        step_monitor_exe_left_step_one_cycle(sleep_time);
    }
}

void step_monitor_exe_right_step_params_cylce(int cycles, int sleep_time){
    for (int i = 0; i < cycles; i++)
    {
        step_monitor_exe_right_step_one_cycle(sleep_time);
    }
}

#pragma region one cycle

void step_monitor_exe_both_step_one_cycle(int sleep_time)
{
    IoTGpioSetOutputVal(MOTOR_LEFT_STEP_GPIO, 1);
    IoTGpioSetOutputVal(MOTOR_RIGHT_STEP_GPIO, 1);
    usleep(sleep_time);
    IoTGpioSetOutputVal(MOTOR_LEFT_STEP_GPIO, 0);
    IoTGpioSetOutputVal(MOTOR_RIGHT_STEP_GPIO, 0);
    usleep(sleep_time);
    MYAPP_LOGD(TAG, "STEP MONITOR EXE *BOTH* 1 CYCLE SUCCESS: code %d", 0);
}

void step_monitor_exe_left_step_one_cycle(int sleep_time)
{
    IoTGpioSetOutputVal(MOTOR_LEFT_STEP_GPIO, 1);
    usleep(sleep_time);
    IoTGpioSetOutputVal(MOTOR_LEFT_STEP_GPIO, 0);
    usleep(sleep_time);
    MYAPP_LOGD(TAG, "STEP MONITOR EXE *LEFT* 1 CYCLE SUCCESS: code %d", 0);
}

void step_monitor_exe_right_step_one_cycle(int sleep_time)
{
    IoTGpioSetOutputVal(MOTOR_RIGHT_STEP_GPIO, 1);
    usleep(sleep_time);
    IoTGpioSetOutputVal(MOTOR_RIGHT_STEP_GPIO, 0);
    usleep(sleep_time);
    MYAPP_LOGD(TAG, "STEP MONITOR EXE *RIGHT* 1 CYCLE SUCCESS: code %d", 0);
}

#pragma endregion